#ifndef __SV2_CAMERA_NODE__
#define __SV2_CAMERA_NODE__

#include "sms_core.h"
#include "sv2_camera_base.h"
#include <opencv2/opencv.hpp>
#include <string>
#include <chrono>
#include <mutex>
#include <queue>
#include <iostream>
#include <chrono>
#include <thread>
#include <unordered_map>

/*
依赖安装
sudo apt install libopencv-dev
sudo apt install libopencv-contrib-dev

*/

namespace sv2
{

  class CameraCppNode : public sms::BaseNode
  {
  public:
    CameraCppNode(
      std::string job_name,
      std::string param_file,
      std::string ip = "127.0.0.1",
      int port = 9094,
      nlohmann::json kwargs = {}
      ) : 
      sms::BaseNode("CameraCppNode", job_name, param_file, ip, port, kwargs),
      _calib_pub("/" + job_name + "/sensor/calibration_info", "sensor_msgs::CameraCalibration"),
      _image_pub("/" + job_name + "/sensor/image_raw", "memory_msgs::RawImage")
    {

      // 读取节点参数
      this->camera_type = this->get_param("camera_type", "V4L2CAM");
      this->fourcc = this->get_param("fourcc", "MJPG");
      this->camera_ip = this->get_param("camera_ip", "192.168.2.64");
      this->camera_id = this->get_param("camera_id", 0);
      this->image_width = this->get_param("width", 640);
      this->image_height = this->get_param("height", 480);
      this->fps = this->get_param("fps", 30);
      this->frame_id = this->get_param("frame_id", "camera");
      this->camera_matrix = this->get_param("camera_matrix", {793.79, 0, 297.83, 0, 794.91, 309.42, 0, 0, 1});
      this->distortion_coefficients = this->get_param("distortion_coefficients", {0.0, 0.0, 0.0, 0.0, 0.0});
      this->projection = this->get_param("projection", {793.79, 0, 297.83, 0, 0, 794.91, 309.42, 0, 0, 0, 1, 0});
      this->params_help();

      logger.info("camera_id: " + std::to_string(this->camera_id));
      logger.info("frame_id: " + this->frame_id);
      logger.info("camera_type: " + this->camera_type);
      logger.info("fourcc: " + this->fourcc);
      logger.info("width: " + std::to_string(static_cast<int>(this->image_width)));
      logger.info("height: " + std::to_string(static_cast<int>(this->image_height)));
      logger.info("fps: " + std::to_string(static_cast<double>(this->fps)));
      logger.info("camera_matrix: " + this->camera_matrix.dump());
      logger.info("distortion_coefficients: " + this->distortion_coefficients.dump());

      // 打开摄像头
      this->cap.setWH(this->image_width, this->image_height);
      this->cap.setFps(this->fps);
      this->cap.setIp(this->camera_ip);
      this->cap.setFourcc(this->fourcc);
      this->cap.open(this->getCameraType(this->camera_type), this->camera_id); // CameraID 0
    }
    ~CameraCppNode()
    {
    }
    void run();
    CameraType getCameraType(const std::string &camera_string);

    cv::Mat img_;
    nlohmann::json camera_matrix;
    nlohmann::json distortion_coefficients;
    nlohmann::json projection;
    std::string frame_id;
    std::string camera_type;
    std::string camera_ip;
    std::string fourcc;
    int image_width;
    int image_height;
    int camera_id;
    double fps;
    Camera cap;

  private:
    // 发布话题
    sms::Publisher _image_pub;
    sms::Publisher _calib_pub;
  };

}
#endif
